/**
 * Copyright (C) 2025 ROKAE (Beijing) Technology Co., LTD.
 * All Rights Reserved.
 *
 * Information in this file is the intellectual property of Rokae Technology Co., Ltd,
 * And may contains trade secrets that must be stored and viewed confidentially.
 *
 * @file ElectricClawRLCmd.cpp
 * @brief 注册 RL 指令
 */
#include "ElectricClawRLCmd.h"

namespace electric_claw_cmd
{
    using namespace xcore_api::rl_cmd;
    using namespace xcore_api::endtool;
    
    /**
     * @brief 实现指令OnrobotStop
     */
    MORDERN_CMD_ARGS_API(OnrobotStop)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，电磁爪的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(OnrobotStop)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取电磁爪ID
            int claw_id = args[0].GetIntVal();
            // 停止电磁爪
            bool ret = api.stopElectricClaw(claw_id);

            LOG(INFO) << "send electric claw stop cmd result:" << ret;
            LOG(INFO) << "claw_id:" << claw_id;
            LOG(INFO) << ":" << GetCmdTask() << " " << GetCmdFile() << " " << GetCmdLine();

            if (ret)
            {
                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };
        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeOnrobotStop()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("OnrobotStop");
        // 设置指令前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::ACTIVATE_AT_LOOKAHEAD);
        // 设置任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 函数的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退的类型
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);

        return std::make_shared<OnrobotStop>(cfg);
    }

    /**
     * @brief 实现指令OnrobotMove
     */
    MORDERN_CMD_ARGS_API(OnrobotMove)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，电磁爪的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的模式 (1: 外部模式， 2: 内部模式)
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的宽度
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的力
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的速度
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(OnrobotMove)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取电磁爪ID
            int claw_id = args[0].GetIntVal();
            // 获取电磁爪模式
            int mode = args[1].GetIntVal();
            // 获取电磁爪宽度
            int width = args[2].GetIntVal();
            // 获取电磁爪力
            int force = args[3].GetIntVal();
            // 获取电磁爪速度
            int speed = args[4].GetIntVal();
            
            if (mode != 1 && mode != 2)
            {
                LOG(ERROR) << "invalid mode: " << mode;
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
            if (width < 160 || width > 685)
            {
                LOG(ERROR) << "invalid width: " << width;
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
            if (force < 0 || force > 140)
            {
                LOG(ERROR) << "invalid force: " << force;
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
            if (speed < 20 || speed > 100)
            {
                LOG(ERROR) << "invalid speed: " << speed;
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }

            bool ret = api.startElectricClaw(
                claw_id, width, force, speed, mode);

            if (ret)
            {
                LOG(INFO) << "send electric claw start cmd result: " << ret;
                LOG(INFO) << "claw_id: " << claw_id;
                LOG(INFO) << "mode: " << static_cast<int>(mode);
                LOG(INFO) << "width: " << static_cast<int>(width);
                LOG(INFO) << "force: " << static_cast<int>(force);
                LOG(INFO) << "speed: " << static_cast<int>(speed);

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeOnrobotMove()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("OnrobotMove");
        // 设置指令前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::WAIT_MOVE_PAUSE);
        // 设置任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 函数的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退的类型
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);

        return std::make_shared<OnrobotMove>(cfg);
    }

    /**
     * @brief 实现指令OnrobotStatus
     */
    MORDERN_CMD_ARGS_API(OnrobotStatus)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，电磁爪的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的状态
        v.emplace_back(ValueTypeAPI::VALUE_INT, true); 
        // 必选参数，电磁爪的外部宽度
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的内部宽度
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，电磁爪的力
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);

        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(OnrobotStatus)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取电磁爪ID
            int claw_id = args[0].GetIntVal();
            
            int status = 0;
            int external_width = 0;
            int internal_width = 0;
            int force = 0;
            
            bool ret = api.getStatusElectricClaw(
                claw_id, status, external_width, internal_width, force);

            if (ret)
            {
                LOG(INFO) << "get electric claw status cmd result: " << ret;
                LOG(INFO) << "claw_id: " << claw_id;
                LOG(INFO) << "status: " << status;
                LOG(INFO) << "external_width: " << external_width;
                LOG(INFO) << "internal_width: " << internal_width;
                LOG(INFO) << "force: " << force;
                
                args[1].ResetInt(status);
                args[2].ResetInt(external_width);
                args[3].ResetInt(internal_width);
                args[4].ResetInt(force);

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeOnrobotStatus()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("OnrobotStatus");
        // 设置指令前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::ACTIVATE_AT_LOOKAHEAD);
        // 设置任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 函数的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退的类型
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);

        return std::make_shared<OnrobotStatus>(cfg);
    }
}

/**
 * @brief 注册所有电磁爪控制指令到系统中
 */
void ElectricClawCmd()
{
    xcore_api::rl_cmd::RegPluginCmdsAPI(electric_claw_cmd::makeOnrobotStop());
    xcore_api::rl_cmd::RegPluginCmdsAPI(electric_claw_cmd::makeOnrobotMove());
    xcore_api::rl_cmd::RegPluginCmdsAPI(electric_claw_cmd::makeOnrobotStatus());
}